#include "Pid.h"

void VALUE_LIMIT(float Input,float Min,float Max)
{
	if(Input > Max)
		Input = Max;
	else if(Input < Min)
		Input = Min;
	else
	  Input = Input;
}


void Pid_Init(Pid_Info_Typedef *Pid,float Param[5])
{
	Pid->Kp = Param[0];
	Pid->Ki = Param[1];
	Pid->Kd = Param[2];
	Pid->Integral_Limit  = Param[3];
	Pid->Limit = Param[4];
}


void Pid_Controller(Pid_Info_Typedef *Pid,float Target,float Masure)
{
	Pid->Error[1] = Pid->Error[0];
	Pid->Error[0] = Target - Masure;
	
	Pid->Integral += Pid->Error[0];
	VALUE_LIMIT(Pid->Integral,-Pid->Integral_Limit,Pid->Integral_Limit);
	
	Pid->Output = Pid->Kp * Pid->Error[0] + Pid->Integral * Pid->Ki + Pid->Kd * (Pid->Error[0] - Pid->Error[1]);
	VALUE_LIMIT(Pid->Output,-Pid->Limit,Pid->Limit);
}